﻿using System.Collections.Generic;

public class IndexedPriority<T>
{
    private List<T> _vecKeys;
    private List<int> _heaps;
    private List<int> _invHeaps;
    private IComparer<T> _comparer;

    private int _size;
    private int _maxSize;

    private void Swap(int a, int b)
    {
        int temp = _heaps[a];
        _heaps[a] = _heaps[b];
        _heaps[b] = temp;

        //change the handles too
        _invHeaps[_heaps[a]] = a;
        _invHeaps[_heaps[b]] = b;
    }

    private void ReorderUpwards(int nd)
    {
        //move up the heap swapping the elements until the heap is ordered
        while (nd > 1 && _comparer.Compare(_vecKeys[_heaps[nd / 2]], _vecKeys[_heaps[nd]]) > 0)
        {
            Swap(nd / 2, nd);

            nd /= 2;
        }
    }

    private void ReorderDownwards(int nd, int HeapSize)
    {
        //move down the heap from node nd swapping the elements until
        //the heap is reordered
        while (2 * nd <= HeapSize)
        {
            int child = 2 * nd;

            //set child to smaller of nd's two children
            if (child < HeapSize && _comparer.Compare(_vecKeys[_heaps[child]], _vecKeys[_heaps[child + 1]]) > 0)
            {
                ++child;
            }

            //if this nd is larger than its child, swap
            if (_comparer.Compare(_vecKeys[_heaps[nd]], _vecKeys[_heaps[child]]) > 0)
            {
                Swap(child, nd);

                //move the current node down the tree
                nd = child;
            }

            else
            {
                break;
            }
        }
    }

    public IndexedPriority(List<T> keys, IComparer<T> comparer, int maxSize)
    {
        _vecKeys = keys;
        _maxSize = maxSize;
        _size = 0;
        _comparer = comparer;

        _heaps = new List<int>(maxSize + 1);
        _invHeaps = new List<int>(maxSize + 1);
        for (int i = 0; i < maxSize + 1; i++)
        {
            _heaps.Add(0);
            _invHeaps.Add(0);
        }
    }

    public bool Empty()
    {
        return _size == 0;
    }

    public void Insert(int idx)
    {
        Utility.Assert(_size+1 <= _maxSize);

        ++_size;

        _heaps[_size] = idx;

        _invHeaps[idx] = _size;

        ReorderUpwards(_size);
    }
    
    public int Pop()
    {
        Swap(1, _size);

        ReorderDownwards(1, _size-1);

        return _heaps[_size--];
    }
    
    public void ChangePriority(int idx)
    {
        ReorderUpwards(_invHeaps[idx]);
    }
}